#ifndef AEB_H
#define AEB_H


#include <nav_msgs/msg/odometry.hpp>
#include <std_msgs/msg/bool.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <geometry_msgs/msg/pose.hpp>
#include <geometry_msgs/msg/pose_array.hpp>
#include <visualization_msgs/msg/marker.hpp>
#include <visualization_msgs/msg/marker_array.hpp>
#include <std_msgs/msg/header.hpp>
#include <std_msgs/msg/string.hpp>
#include <std_msgs/msg/u_int8.hpp>
#include <std_msgs/msg/int8.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <std_srvs/srv/set_bool.hpp>
#include <tf2/utils.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/buffer.h>
#include <tf2/convert.h>
#include "tf2_sensor_msgs/tf2_sensor_msgs.hpp"
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/filters/crop_box.h>
#include <cstdlib>
#include <map>
#include <ubt_interface/srv/query_aeb_status.hpp>
#include <ubt_interface/srv/set_speed_limit.hpp>
#include <ubt_interface/srv/set_footprint.hpp>
#include <ubt_interface/srv/set_stop_distance.hpp>
#include <ubt_interface/msg/status.hpp>
#include <ubt_interface/srv/set_enable.hpp>
#include <std_srvs/srv/set_bool.hpp>

enum aeb_type{ OTHER, DECELERATION, SPEED_STOP, EMERGENCY_STOP};
static const char *aeb_type_str[] = { "OTHER", "DECELERATION", "SPEED_STOP", "EMERGENCY_STOP" };

class AEBNode
{
public:
  using PointT = pcl::PointXYZI;

  struct ObstacleRange {
    bool exist_obstacle = false; //是否存在障碍物
    aeb_type type = OTHER;
    int range_index = 0;
    double distance = 999.9;
    PointT obstacle_point;
  };

  struct HoldingStatus {
    bool is_holding = false; //是否需要保持状态
    aeb_type type = OTHER;
    geometry_msgs::msg::Twist twist;
    double linear_speed = 999.9;
    double obstacle_distance = 999.9;
    rclcpp::Time start_time;
  };

  struct SpeedLimit {
    double speed_limit = 999.9;
    double angular_limit = 999.9;
  };

  AEBNode();

private:
  rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr predict_path_pub_;
  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr predict_stop_range_pub_;
  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr predict_deceleration_range_pub_;
  rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr emergency_stop_pub_;
  rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_pub_;
  rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr obstacle_range_pub_; //应急范围发布
  rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr obstacles_pub_; //应急障碍发布
  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr status_pub_;
  rclcpp::Publisher<std_msgs::msg::Int8>::SharedPtr obs_status_pub_;
  rclcpp::Publisher<ubt_interface::msg::Status>::SharedPtr params_status_pub_;

  rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr obstacles_sub_;
  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr feedback_sub_;
  rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_sub_;

  rclcpp::Service<ubt_interface::srv::SetEnable>::SharedPtr enable_aeb_server;
  rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr enable_aeb_forever_server;
  rclcpp::Service<ubt_interface::srv::SetSpeedLimit>::SharedPtr speed_limit_server;
  rclcpp::Service<ubt_interface::srv::SetFootprint>::SharedPtr footprint_server;
  rclcpp::Service<ubt_interface::srv::QueryAebStatus>::SharedPtr query_server;
  rclcpp::Service<ubt_interface::srv::SetStopDistance>::SharedPtr stop_distance_server;

  rclcpp::TimerBase::SharedPtr detection_timer;
  std::shared_ptr<tf2_ros::TransformListener> tfl_;
  std::unique_ptr<tf2_ros::Buffer> tf_;

  //params
  pcl::PointCloud<PointT> points_msg_now;
  bool bool_enable_aeb, enable_auto_start_aeb_;
  std::string feedback_cmd_vel_topic, obstacles_topic, cmd_vel_topic, pub_cmd_vel_topic;
  std::string global_frame_, base_link_frame_;
  double feedback_valid_speed_limit, feedback_valid_angular_limit ,straight_angle_limit;
  double deceleration_, acceleration_, angle_deceleration_;
  double  expansion_range_;
  std::vector<pcl::PointXY> footprint;
  std::vector<pcl::PointXY> first_footprint;
  std::vector<pcl::PointXY> emergency_stop_footprint;
  double dis_spacing_, angle_spacing_;
  double holding_time_;
  double aeb_obstacle_angular_speed_;
  double disable_aeb_keeping_time_;
  rclcpp::Time start_holding_time;
  rclcpp::Time start_disable_aeb_time;
  double feedback_valid_time_limit;
  double stop_distance_; //刹车距离
  double stop_angular_;

  double delay_time_;
  int stop_detecetion_threshold_;
  int deceleration_detecetion_threshold_;
  uint8_t emergency_detecetion_number = 0;
  uint8_t stop_detecetion_number = 0;
  uint8_t deceleration_detecetion_number = 0;

  bool io_obstacle_ = false;

  //variable
  bool bool_emergency_stop;
  std::mutex points_msg_now_mutex;
  std::mutex stop_range_list_mutex;
  std::vector<std::vector<geometry_msgs::msg::Point >> stop_range_list;
  std::vector<std::vector<geometry_msgs::msg::Point >> deceleration_range_list;
  std::vector<double> aeb_obstacle_distance;
  std::vector<double> aeb_obstacle_speed;
  ObstacleRange obstacle_range;
  HoldingStatus holding_status;
  SpeedLimit speed_limit;
  geometry_msgs::msg::Twist org_control;
  rclcpp::Time last_control_time;
  geometry_msgs::msg::Twist current_feedback;

  //speed_smooth
  bool enable_smooth_;
  rclcpp::Time last_smooth_time;

  //function callback
  void feedback_callback(const nav_msgs::msg::Odometry::SharedPtr odom);
  void detection_timer_callback();
  void obstacles_callback(const sensor_msgs::msg::PointCloud2::SharedPtr points_msg);
  void cmd_vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg);

  //function service
  bool enable_aeb(ubt_interface::srv::SetEnable::Request::ConstSharedPtr req, ubt_interface::srv::SetEnable::Response::SharedPtr res);
  bool enable_aeb_forever(std_srvs::srv::SetBool::Request::ConstSharedPtr req, std_srvs::srv::SetBool::Response::SharedPtr res);
  bool set_speed_limit(ubt_interface::srv::SetSpeedLimit::Request::ConstSharedPtr req, ubt_interface::srv::SetSpeedLimit::Response::SharedPtr res);
  bool set_footprint(ubt_interface::srv::SetFootprint::Request::ConstSharedPtr req, ubt_interface::srv::SetFootprint::Response::SharedPtr res);
  bool set_stop_distance(ubt_interface::srv::SetStopDistance::Request::ConstSharedPtr req, ubt_interface::srv::SetStopDistance::Response::SharedPtr res);
  bool query_aeb_status(ubt_interface::srv::QueryAebStatus::Request::ConstSharedPtr req, ubt_interface::srv::QueryAebStatus::Response::SharedPtr res);

  //function
  void publish_predict_path(geometry_msgs::msg::PoseArray pose_array);
  void publish_predict_stop_range(geometry_msgs::msg::PoseArray pose_array);
  void publish_predict_deceleration_range(geometry_msgs::msg::PoseArray pose_array);
  std::vector<geometry_msgs::msg::Point> get_rect(geometry_msgs::msg::Pose pose, double expansion = 0.0);
  bool pt_in_any_rect(geometry_msgs::msg::Point pCur, geometry_msgs::msg::Point pLT, geometry_msgs::msg::Point pLB, geometry_msgs::msg::Point pRB,geometry_msgs::msg::Point pRT);
  void publish_emergency_range(geometry_msgs::msg::Point obstacle, std::vector<geometry_msgs::msg::Point> RectPoints);
  void publish_deceleration_range(geometry_msgs::msg::Point obstacle, std::vector<geometry_msgs::msg::Point> RectPoints);
  bool get_footprint_from_param();
  bool get_emergency_stop_footprint_from_param();
  bool get_speed_limit_param();
  double get_speed_limit(ObstacleRange obstacle_range);
  bool check_need_holding(double speed = 999.9);
  geometry_msgs::msg::Twist cal_twist(geometry_msgs::msg::Twist org, double limit_speed, bool need_holding);
  geometry_msgs::msg::Twist cal_twist_by_limits(geometry_msgs::msg::Twist org, SpeedLimit limits);
  geometry_msgs::msg::Twist speed_smooth(geometry_msgs::msg::Twist org); //速度平滑
  bool check_emergency_stop();
  void publish_range_marker(std::vector<geometry_msgs::msg::Point> range, aeb_type type);
  void publish_obstacle_marker(PointT point, aeb_type type);
  std_msgs::msg::ColorRGBA get_color(aeb_type type);
  void publish_status(aeb_type type);
  double cal_linear_speed(double x, double y);
  void set_holding_status(bool is_holding, aeb_type type, geometry_msgs::msg::Twist twist, double linear_speed = 999.9, double obstacle_distance = 999.9);
  void publish_params_status();
};
#endif // AEB_H
